import numpy as np
import cv2

#blocksize窗口大小；
#ksize算梯度的时候用的那个东西的大小，这里用的sobel是3*3
def cornerHarris(img, blocksize=2, ksize=3, k=0.04):
    #下面是一个闭包
    def _clacHarris(cov, k):
        #numpy.zeros
        #创建指定大小的数组，数组元素以 0 来填充：
        result = np.zeros([cov.shape[0], cov.shape[1]], dtype=np.float32)
        for i in range(cov.shape[0]):
            for j in range(cov.shape[1]):
                a = cov[i, j, 0]
                b = cov[i, j, 1]
                c = cov[i, j, 2]
                result[i, j] = a*c-b*b-k*(a+c)*(a+c)
        return result

    #算图像竖直和水平方向的梯度
    Dx = cv2.Sobel(img, cv2.CV_32F, 1, 0, ksize=ksize)
    Dy = cv2.Sobel(img, cv2.CV_32F, 0, 1, ksize=ksize)

    #img.shape 图像数组的大小（行、列、颜色通道）
    cov = np.zeros([img.shape[0], img.shape[1], 3], dtype=np.float32)

    for i in range(img.shape[0]):
        for j in range(img.shape[1]):
            cov[i, j, 0] = Dx[i, j]*Dx[i, j]
            cov[i, j, 1] = Dx[i, j]*Dy[i, j]
            cov[i, j, 2] = Dy[i, j]*Dy[i, j]

    #算块内的梯度和w（x，y）,boxFilter换成高斯效果会好一点
    #cov = cv2.boxFilter(cov, -1, (blocksize, blocksize), normalize=False)
    #高斯滤波
    cov = cv2.GaussianBlur(cov, (9, 9), 0)


    return _clacHarris(cov, k)


if __name__ == '__main__':
    img = cv2.imread('YOIMIYA.jpg')
    # 最近邻插值法缩放
    # 缩放到原来的四分之一
    img = cv2.resize(img, (0, 0), fx=0.5, fy=0.5, interpolation=cv2.INTER_NEAREST)
    gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)#把RGB图转成灰度图
    result = cornerHarris(gray_img, 2, 3, 0.04)
    #角点检测
    pos = cv2.goodFeaturesToTrack(image=result, maxCorners=1000, qualityLevel=0.1, minDistance=1, useHarrisDetector=True, k=0.04)
    for i in range(len(pos)):
        #根据给定的圆心和半径等画圆cv2.circle(img, center, radius, color[, thickness[, lineType[, shift]]])
        cv2.circle(img=img, center=(int(pos[i][0][0]),int( pos[i][0][1])), radius=2, color=[0, 0, 255], thickness=3)

    cv2.imshow('harris', img)
    cv2.waitKey(0)
